#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import Twist
from PyQt5.QtWidgets import (
    QApplication,
    QWidget,
    QVBoxLayout,
    QHBoxLayout,
    QSlider,
    QLineEdit,
    QPushButton,
    QLabel,
)
from PyQt5.QtCore import Qt, QTimer


class NavController(QWidget):
    def __init__(self):
        super().__init__()
        rospy.init_node("cmd_controller_node", anonymous=False)
        self.rate = rospy.Rate(20)  # 20Hz

        # 初始化速度值
        self.linear_x = 0.0
        self.linear_y = 0.0
        self.angular_z = 0.0

        # 默认话题
        self.topic_name = "/cmd_vel"

        # 创建发布者
        self.cmd_pub = rospy.Publisher(self.topic_name, Twist, queue_size=10)

        # 创建界面
        self.init_ui()

        # 定时器用于ROS发布
        self.timer = QTimer()
        self.timer.timeout.connect(self.publish_twist)
        self.timer.start(int(1000 / 20))  # 每 50ms 触发一次

    def init_ui(self):
        self.setWindowTitle("ROS Velocity Controller")

        layout = QVBoxLayout()

        # 文本框（话题）
        self.topic_input = QLineEdit(self.topic_name)
        layout.addWidget(QLabel("Topic:"))
        layout.addWidget(self.topic_input)

        # 滑动条：x速度
        self.x_slider = QSlider(Qt.Vertical)
        self.x_slider.setRange(-50, 50)  # 十倍精度
        self.x_slider.setValue(0)
        layout.addWidget(QLabel("X Speed (-5.0 ~ 5.0)"))
        layout.addWidget(self.x_slider)

        # 滑动条：y速度
        h_layout_y = QHBoxLayout()
        self.y_slider = QSlider(Qt.Horizontal)
        self.y_slider.setRange(-40, 40)
        self.y_slider.setValue(0)
        h_layout_y.addWidget(QLabel("Y Speed (-4.0 ~ 4.0)"))
        h_layout_y.addWidget(self.y_slider)
        layout.addLayout(h_layout_y)

        # 滑动条：角速度
        h_layout_yaw = QHBoxLayout()
        self.yaw_slider = QSlider(Qt.Horizontal)
        self.yaw_slider.setRange(-30, 30)
        self.yaw_slider.setValue(0)
        h_layout_yaw.addWidget(QLabel("Angular Z (-3.0 ~ 3.0)"))
        h_layout_yaw.addWidget(self.yaw_slider)
        layout.addLayout(h_layout_yaw)

        # 清除按钮
        self.clear_button = QPushButton("Clear")
        self.clear_button.clicked.connect(self.clear_sliders)
        layout.addWidget(self.clear_button)

        self.setLayout(layout)

    def keyPressEvent(self, event):
        if event.key() == Qt.Key_W:
            self.x_slider.setValue(self.x_slider.value() + 1)
        elif event.key() == Qt.Key_S:
            self.x_slider.setValue(self.x_slider.value() - 1)
        elif event.key() == Qt.Key_A:
            self.y_slider.setValue(self.y_slider.value() + 1)
        elif event.key() == Qt.Key_D:
            self.y_slider.setValue(self.y_slider.value() - 1)
        elif event.key() == Qt.Key_Q:
            self.yaw_slider.setValue(self.yaw_slider.value() + 1)
        elif event.key() == Qt.Key_E:
            self.yaw_slider.setValue(self.yaw_slider.value() - 1)
        elif event.key() == Qt.Key_Space:
            self.clear_sliders()

    def clear_sliders(self):
        self.x_slider.setValue(0)
        self.y_slider.setValue(0)
        self.yaw_slider.setValue(0)

    def publish_twist(self):
        # 更新速度值
        self.linear_x = self.x_slider.value() / 10.0
        self.linear_y = self.y_slider.value() / 10.0
        self.angular_z = self.yaw_slider.value() / 10.0

        # 动态更新话题
        new_topic = self.topic_input.text()
        if new_topic != self.topic_name:
            self.cmd_pub.unregister()
            self.cmd_pub = rospy.Publisher(new_topic, Twist, queue_size=10)
            self.topic_name = new_topic

        # 发布 Twist 消息
        twist = Twist()
        twist.linear.x = self.linear_x
        twist.linear.y = self.linear_y
        twist.angular.z = self.angular_z
        self.cmd_pub.publish(twist)

    def run(self):
        self.show()
        while not rospy.is_shutdown():
            self.rate.sleep()
            QApplication.processEvents()


if __name__ == "__main__":
    import sys

    app = QApplication(sys.argv)
    controller = NavController()
    controller.run()
